#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray, String
import numpy as np
import argparse

def main():
    parser = argparse.ArgumentParser(description='Send pose commands repeatedly.')
    parser.add_argument('--repeat', type=int, default=1,
                        help='Number of times to toggle pose commands')
    args = parser.parse_args()

    rospy.init_node('move_pose_commander', anonymous=True)
    pub_joint = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)
    pub_pose = rospy.Publisher('/command_move_pose', Float64MultiArray, queue_size=10)

    rospy.sleep(1)

    msg = Float64MultiArray()
    start_joint = [0.5319671630859375, 0.3942546844482422, 0.50946044921875, -0.831425666809082,
                   1.6386289596557617, -0.9843978881835938, -2.03688907623291]

    print("move to start point...")
    msg.data = start_joint
    pub_joint.publish(msg)
    rospy.sleep(5.5)

    for i in range(args.repeat):
        print(f"[{i+1}/{args.repeat}] move pose lower...")
        msg.data = [0.33, 0.15, 0.32, 0.0, 0.707, 0.0, 0.707]
        pub_pose.publish(msg)
        rospy.sleep(2)

        print(f"[{i+1}/{args.repeat}] move pose higher...")
        msg.data = [0.33, 0.15, 0.52, 0.0, 0.707, 0.0, 0.707]
        pub_pose.publish(msg)
        rospy.sleep(2)

if __name__ == '__main__':
    main()
